// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningAndTrackingManifest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using Microsoft.Kinect;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.EV4.Runtime;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Navigation.Visualization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Visualization;
    using Test.Robotics.Hardware.EV4;
    using Test.Robotics.Interaction;
    using Test.Robotics.Metrics;
    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;
    using Test.Robotics.Vision;

    /// <summary>
    /// Provides methods for creating manifests in a type-checked way.
    /// </summary>
    public static class PathPlanningAndTrackingManifest
    {
        /// <summary>
        /// Radius of the robot.
        /// </summary>
        private const double Ev4RobotRadius = 0.16;

        /// <summary>
        /// Distance in meters to track ahead along the path.
        /// </summary>
        private const double TrackAheadDistanceInMeters = 0.75;

        /// <summary>
        /// Adds the path planning agent and corresponding UI.
        /// </summary>
        /// <param name="manifest">The manifest to which to add agents and models.</param>
        /// <param name="views">The list to which to add views.</param>
        /// <param name="radiusInMeters">Robot radius in meters (may be padded to account for deviation, etc.).</param>
        /// <param name="minObstacleSizeInMeters">Minimum obstacle size in meters.</param>
        /// <param name="maxObstacleDistanceInMeters">Maximum obstacle distance in meters.</param>
        /// <param name="planner">Path planning algorithm.</param>
        /// <param name="mapProducer">Global map producer.</param>
        /// <returns>The path planning agent.</returns>
        public static PathPlanningAgent AddPathPlanning(Manifest manifest, List<ViewDefinition> views, double radiusInMeters, double minObstacleSizeInMeters, double maxObstacleDistanceInMeters, IGridBasedPathPlanner planner, IProducer<SLAM2DAgentMessage> mapProducer)
        {
            manifest.OtherKnownTypes.Add(typeof(PathPlanner));

            var pathPlanning = new PathPlanningAgent(
                name: "PathPlanning",
                planner: planner,
                radiusInMeters: radiusInMeters,
                corridorBuffer: (int)(radiusInMeters / GlobalMap.DefaultDistanceResolution * 2.5 + 0.5),
                skeletonThreshold: (int)(radiusInMeters / GlobalMap.DefaultDistanceResolution * 2 + 0.5),
                minObstacleSizeInMeters: minObstacleSizeInMeters,
                maxObstacleDistanceInMeters: maxObstacleDistanceInMeters,
                globalMapProducer: mapProducer);
            manifest.Agents.Add(pathPlanning);

            var pathPlanningAndTrackingModel = new PathPlanningAndTrackingModelAgent(
                name: "PathPlanningAndTrackingModelAgent",
                pathPlanProducerAgent: pathPlanning,
                maxUpdateIntervalMilliseconds: 0,
                targetStepsAhead: (int)(TrackAheadDistanceInMeters / GlobalMap.DefaultDistanceResolution + 0.5));
            manifest.Agents.Add(pathPlanningAndTrackingModel);

            var pathPlanningAndTrackingView = new ViewDefinition(
                name: "PathPlanningView",
                viewType: typeof(PathPlanningAndTrackingView),
                modelAgent: pathPlanningAndTrackingModel);
            views.Add(pathPlanningAndTrackingView);

            return pathPlanning;
        }

        /// <summary>
        /// Adds the path tracking agent.
        /// </summary>
        /// <remarks>The planning/tracking UI is added by AddPathPlanning.</remarks>
        /// <param name="manifest">The manifest to which to add agents and models.</param>
        /// <param name="views">The list to which to add views.</param>
        /// <param name="pathPlanning">Path planning agent.</param>
        /// <returns>The path tracking agent.</returns>
        public static PathTrackingAgent AddPathTracking(Manifest manifest, List<ViewDefinition> views, IProducer<SimpleAgentMessage<PathPlan>> pathPlanning)
        {
            var pathTracking = new PathTrackingAgent(
                name: "PathTracking",
                planner: pathPlanning,
                maxDistancePerDriveCommand: 0.1,
                mapResolution: GlobalMap.DefaultDistanceResolution,
                trackAheadInMeters: TrackAheadDistanceInMeters);
            manifest.Agents.Add(pathTracking);
            return pathTracking;
        }

        /// <summary>
        /// Adds a remoting proxy to broadcast drive direction messages over the network.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="pathTracking">The agent to network-enable.</param>
        /// <param name="targetNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <returns>A remoting proxy</returns>
        public static ProxyDefinition AddPathTrackingNetworkEnabledProxy(Manifest manifest, IProducer<SimpleAgentMessage<DriveDirection>> pathTracking, string targetNameOrAddress)
        {
            ProxyDefinition pathTrackingProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: pathTracking.Name + "Proxy",
                producer: pathTracking.Name,
                address: targetNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<DriveDirection>) },
                publishNetworkPortNumber: (int)RemotingPorts.PathTrackingPublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.NetworkEnabledAgentProxies.Add(pathTrackingProxy);

            return pathTrackingProxy;
        }

        /// <summary>
        /// Adds the PathTrackingDrive agent.
        /// </summary>
        /// <param name="manifest">The manifest to which to add agents.</param>
        /// <param name="views">The list to which to add views.</param>
        /// <param name="driveDirectionProducer">The DriveDirection agent.</param>
        /// <param name="poseProducer">The pose agent.</param>
        /// <param name="hdpProducer">Horizontal depth profile producer.</param>
        /// <param name="driveFeedbackProducer">The drive feedback agent.</param>
        /// <returns>The PathTrackingDrive agent.</returns>
        public static PathTrackingDriveAgent AddPathTrackingDrive(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<SimpleAgentMessage<DriveDirection>> driveDirectionProducer,
            IProducer<SimpleAgentMessage<Pose2D>> poseProducer,
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> hdpProducer,
            IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedbackProducer)
        {
            var pathTrackingDrive = new PathTrackingDriveAgent(
                name: "PathTrackingDrive",
                driveDirectionProducer: driveDirectionProducer,
                poseProducer: poseProducer,
                hdpProducer: hdpProducer,
                driveFeedbackProducer: driveFeedbackProducer,
                maxTurnRate: 0.5,
                backingTurnRateFactor: 0.3,
                maxSpeed: 1,
                turnGain: 0.95,
                headingDampening: 5,
                forwardAccelerationFactor: 0.04,
                forwardDecelerationFactor: -0.08,
                turnAccelerationFactor: 0.05,
                turnDecelerationFactor: -0.08,
                obstacleAvoidanceDistanceThreshold: 0.6,
                obstacleAvoidanceHeadingAdjustmentGain: 0.05,
                obstacleAvoidanceMaximumHeadingAdjustment: 0.6,
                corridorRadius: Ev4RobotRadius + 0.3,
                deadmanTriggerMilliseconds: 1000);
            manifest.Agents.Add(pathTrackingDrive);

            return pathTrackingDrive;
        }

        /// <summary>
        /// Adds the Ev4LocalSuggestionDrive agent.
        /// </summary>
        /// <param name="manifest">The manifest to which to add agents.</param>
        /// <param name="views">The list to which to add views.</param>
        /// <param name="velocitySuggestionProducer">The agent that actually suggests driving.</param>
        /// <param name="kinectProducer">The Kinect agent.</param>
        /// <param name="sonarProducer">Sonar agent.</param>
        /// <returns>Ev4LocalSuggestionDriveAgent agent.</returns>
        public static Ev4LocalSuggestionDriveAgent AddEv4LocalSuggestionDrive(
            Manifest manifest,
            List<ViewDefinition> views,
            IProducer<MobileBaseVelocityReferenceMessage> velocitySuggestionProducer,
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinectProducer,
            IProducer<SonarMessage> sonarProducer)
        {
            var suggestionDrive = new Ev4LocalSuggestionDriveAgent(
                name: "Ev4LocalSuggestionDrive",
                producer: velocitySuggestionProducer.Name,
                kinectProducerAgent: kinectProducer,
                sonarProducerAgent: sonarProducer,
                sonarThresholdDistanceInMM: 400,
                depthThresholdInMM: 540,
                escapeRotationRadsPerSecMin: 0.3,
                escapeRotationRadsPerSecMax: 1.5,
                escapeRotationRadsPerSecVariance: 0.25,
                desiredOverShootMillisecondsWhileTurning: 250,
                maxMillisecondsDrivingForwardAfterTurning: 1000,
                driveForwardSpeedAfterAvoiding: 0.5,
                probabilityOfRespectingSuggestedDirection: 0.3);

            manifest.Agents.Add(suggestionDrive);

            return suggestionDrive;
        }

        /// <summary>
        /// Adds a desktop proxy for a remote PathTracking agent running on the robot.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="views">The set of views to add to</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot</param>
        /// <returns>The PathTracking proxy</returns>
        public static IProducer<SimpleAgentMessage<DriveDirection>> AddPathTrackingRemoteProxy(Manifest manifest, List<ViewDefinition> views, string robotNameOrAddress)
        {
            // now create a proxy with the same name and add it to the manifest
            ProxyDefinition remoteProxy = ProxyDefinition.CreateRemoteProxy(
                name: "PathTrackingDriveAgent",
                address: robotNameOrAddress,
                publishContracts: new[] { typeof(SimpleAgentMessage<DriveDirection>) },
                publishNetworkPortNumber: (int)RemotingPorts.PathTrackingPublish,
                controlContracts: null,
                controlNetworkPortNumber: 0);
            manifest.RemoteAgentProxies.Add(remoteProxy);

            return remoteProxy.AsProducer<SimpleAgentMessage<DriveDirection>>();
        }

        /// <summary>
        /// Creates a manifest for running path tracking on the controller (desktop/laptop).
        /// </summary>
        /// <param name="robotNameOrAddress">The name or IP address of the robot to broadcast to which to connect.</param>
        /// <param name="robotConfigFolder">Folder in which to find calibration files.</param>
        /// <param name="addLocalizationFn">Function to "splice" in SLAM, static localization, etc.</param>
        /// <param name="addPathTrackingFn">Function to "splice" in path tracking, with/without metrics, etc.</param>
        /// <param name="addUI">Whether to include visualizations.</param>
        /// <returns>A manifest to run path tracking on the controller (desktop/laptop).</returns>
        public static Manifest CreatePathPlanningManifest(
            string robotNameOrAddress,
            string robotConfigFolder,
            Func<Manifest, List<ViewDefinition>, IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>>, IProducer<SimpleAgentMessage<DriveFeedbackState>>, IProducer<SLAM2DAgentMessage>> addLocalizationFn, // man I miss *automatic* generics and type inference in F#...
            Func<Manifest, List<ViewDefinition>, IProducer<SimpleAgentMessage<PathPlan>>, IProducer<SimpleAgentMessage<DriveFeedbackState>>, IProducer<BatteryMessage>, IProducer<SimpleAgentMessage<DriveDirection>>> addPathTrackingFn, // really, I miss it...
            bool addUI = true)
        {
            Manifest manifest = new Manifest();
            var views = new List<ViewDefinition>();

            // Consume drive feedback and raw Kinect. Produce path tracking and localization.
            IProducer<SimpleAgentMessage<KinectHorizontalDepthProfile>> hdpProxy = NavigationManifest.AddKinectHorizontalDepthProfileRemoteProxy(manifest, views, robotNameOrAddress);
            IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedbackProxy = NavigationManifest.AddDriveFeedbackRemoteProxy(manifest, views, robotNameOrAddress);
            IProducer<SLAM2DAgentMessage> localization = addLocalizationFn(manifest, views, hdpProxy, driveFeedbackProxy);
            SlamPoseToMapPoseAgent robotPose = new SlamPoseToMapPoseAgent("RobotMapPoseAgent", localization);
            manifest.Agents.Add(robotPose);
            manifest.ActiveAgents.Add(robotPose.Name);
            PathPlanningAgent pathPlanning = PathPlanningAndTrackingManifest.AddPathPlanning(
                manifest: manifest,
                views: views,
                radiusInMeters: Ev4RobotRadius + 0.04, // padded to allow for path deviation
                minObstacleSizeInMeters: 0.1,
                maxObstacleDistanceInMeters: 5.0,
                planner: new PathPlanner(),
                mapProducer: localization);
            IProducer<BatteryMessage> batteryProxy = EV4Manifest.AddEV4RemoteProxy(manifest, views, robotNameOrAddress, robotConfigFolder);
            manifest.ActiveAgents.Add(batteryProxy.Name);
            IProducer<SimpleAgentMessage<DriveDirection>> pathTracking = addPathTrackingFn(manifest, views, pathPlanning, driveFeedbackProxy, batteryProxy);
            manifest.ActiveAgents.Add(pathTracking.Name);
            ProxyDefinition pathTrackingProxy = PathPlanningAndTrackingManifest.AddPathTrackingNetworkEnabledProxy(manifest, pathTracking, robotNameOrAddress);
            manifest.ActiveAgents.Add(pathTrackingProxy.Name);

            // remoting pose
            ProxyDefinition robotPoseProxy = NavigationManifest.AddRobotMapPoseNetworkEnabledProxy(manifest, robotPose, robotNameOrAddress);
            manifest.ActiveAgents.Add(robotPoseProxy.Name);

            // scaled down kinect
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> scaledDownKinectProxy = VisionManifest.AddDownscaledDepthFrameProducerProxy(manifest, views, robotNameOrAddress);
            
            if (addUI)
            {
                GamePadEmulatorAgent gamePad = InteractionManifest.AddGamePadEmulatorAgent(manifest, views);
                manifest.ActiveAgents.Add(gamePad.Name);
                ProxyDefinition gamePadProxy = InteractionManifest.AddNetworkEnabledGamePadAgentProxy(manifest, views, gamePad, robotNameOrAddress);
                manifest.ActiveAgents.Add(gamePadProxy.Name); 
                
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            RuntimeManifest.AddRecorder<SimpleAgentMessage<KinectHorizontalDepthProfile>>(hdpProxy, manifest);
            RuntimeManifest.AddRecorder<SimpleAgentMessage<DriveFeedbackState>>(driveFeedbackProxy, manifest);
            RuntimeManifest.AddRecorder<SimpleAgentMessage<DriveDirection>>(pathTracking, manifest);
            RuntimeManifest.AddRecorder<SLAM2DAgentMessage>(localization, manifest);
            RuntimeManifest.AddRecorder<DepthCameraAgentMessage<KinectDepthCameraParameters>>(scaledDownKinectProxy, manifest);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest for running path tracking with static localization on the controller (desktop/laptop).
        /// </summary>
        /// <param name="robotNameOrAddress">The name or IP address of the robot to broadcast to which to connect.</param>
        /// <param name="robotConfigFolder">Folder in which to find calibration files.</param>
        /// <param name="mapFile">The map to load.</param>
        /// <param name="startPose">The start pose of the robot.</param>
        /// <returns>A manifest to run path tracking on the controller (desktop/laptop).</returns>
        public static Manifest CreatePathPlanningWithSLAMManifest(string robotNameOrAddress, string robotConfigFolder, string mapFile, Pose2D startPose)
        {
            return CreatePathPlanningManifest(
                robotNameOrAddress,
                robotConfigFolder,
                (manifest, views, hdp, driveFeedback) =>
                    NavigationManifest.AddSLAM2D(manifest, views, hdp, driveFeedback, mapFile, startPose, robotConfigFolder, DepthImageConstants.MaximumRange),
                (manifest, views, pathPlanning, _, __) =>
                    PathPlanningAndTrackingManifest.AddPathTracking(manifest, views, pathPlanning),
                true);
        }

        /// <summary>
        /// Creates a manifest for running path tracking with SLAM on the controller (desktop/laptop).
        /// </summary>
        /// <param name="robotNameOrAddress">The name or IP address of the robot to broadcast to which to connect.</param>
        /// <param name="robotConfigFolder">Folder in which to find calibration files.</param>
        /// <param name="mapFile">The map to load.</param>
        /// <param name="startPose">The start pose of the robot.</param>
        /// <param name="startPoseVariance">The ambiguity in the start pose.</param>
        /// <returns>A manifest to run path tracking on the controller (desktop/laptop).</returns>
        public static Manifest CreatePathPlanningWithStaticLocalizationManifest(string robotNameOrAddress, string robotConfigFolder, string mapFile, Pose2D startPose, Pose2D startPoseVariance)
        {
            return CreatePathPlanningManifest(
                robotNameOrAddress,
                robotConfigFolder,
                (manifest, views, hdp, driveFeedback) =>
                    NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(manifest, views, hdp, driveFeedback, mapFile, startPose, startPoseVariance, robotConfigFolder),
                (manifest, views, pathPlanning, _, __) =>
                    PathPlanningAndTrackingManifest.AddPathTracking(manifest, views, pathPlanning),
                true);
        }

        /// <summary>
        /// Add control proxy agent.
        /// </summary>
        /// <param name="manifest">Manifest to which to add.</param>
        /// <param name="producer">Producer agent name.</param>
        /// <param name="controllerNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <param name="controlMessageType">Control contract message type.</param>
        /// <param name="remotingPort">Control network port number.</param>
        /// <returns>Control proxy definition.</returns>
        public static ProxyDefinition AddControlProxy(Manifest manifest, string producer, string controllerNameOrAddress, Type controlMessageType, int remotingPort)
        {
            var proxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: producer + "ControlProxy",
                producer: producer,
                address: controllerNameOrAddress,
                publishContracts: null,
                publishNetworkPortNumber: 0,
                controlContracts: new[] { controlMessageType },
                controlNetworkPortNumber: remotingPort);
            manifest.NetworkEnabledAgentProxies.Add(proxy);

            return proxy;
        }

        /// <summary>
        /// Creates a manifest for running path tracking on the robot.
        /// </summary>
        /// <param name="controllerNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <param name="depthFormat">Depth image format.</param>
        /// <param name="robotConfigFolder"> Ev4 calibration folder path</param>
        /// <param name="environmentConfigFolder">Environment config folder path</param>
        /// <param name="useUpFacingCamera">Boolean indicating if the up facing camera should be added</param>
        /// <returns>A manifest to run path tracking on the robot.</returns>
        public static Manifest CreatePathTrackingManifest(string controllerNameOrAddress, DepthImageFormat depthFormat, string robotConfigFolder, string environmentConfigFolder, bool useUpFacingCamera)
        {
            // Consume path tracking and SLAM2D from the controller; directing path tracking drive.
            // Produce HDP and drive feedback to the controller.
            Manifest manifest = new Manifest();
            var views = new List<ViewDefinition>();

            // heartbeat remoting
            HeartbeatAgent heartbeat = EV4Manifest.AddEV4HeartbeatAgent(manifest, views, robotConfigFolder);
            ProxyDefinition heartbeatProxy = EV4Manifest.AddEV4HeartbeatNetworkEnabledProxy(manifest, heartbeat, controllerNameOrAddress);
            manifest.ActiveAgents.Add(heartbeatProxy.Name);

            // Kinect/HDP
            KinectAgent kinectRaw = VisionManifest.AddEV4Kinect(manifest, views, depthFormat, true);
            TurretScannerAgent turretScanner = NavigationManifest.AddTurretScanner(manifest, views, robotConfigFolder, heartbeat, kinectRaw);
            ProxyDefinition turretScannerControlProxy = PathPlanningAndTrackingManifest.AddControlProxy(manifest, turretScanner.Name, controllerNameOrAddress, typeof(TurretScanInitiationMessage), (int)RemotingPorts.TurretScannerControl);
            manifest.ActiveAgents.Add(turretScannerControlProxy.Name);
            KinectAgentMessageProcessor kinect = VisionManifest.AddKinectProcessor(manifest, views, turretScanner, robotConfigFolder, 1000);
            JointMessageInterpolatorAgent jointMessageInterpolator = NavigationManifest.AddJointMessageInterpolator(manifest, views, heartbeat, kinect);
            KinectPoseUpdaterAgent kinectPose = NavigationManifest.AddKinectPoseUpdater(manifest, views, kinect, jointMessageInterpolator);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinectPose, robotConfigFolder);
            ProxyDefinition hdpProxy = NavigationManifest.AddKinectHorizontalDepthProfileNetworkEnabledProxy(manifest, hdp, controllerNameOrAddress);
            manifest.ActiveAgents.Add(hdpProxy.Name);            

            // Kinect downscaling for visualization on the controller machine
            AddDepthFrameDownscalerAgent(manifest, kinect, controllerNameOrAddress, 4, 5);

            // drive remoting
            DifferentialDriveMessageInterpolatorAgent differentialDriveMessageInterpolator = NavigationManifest.AddDifferentialDriveMessageInterpolator(manifest, views, heartbeat, kinect);
            DifferentialDriveAgent diffDrive = NavigationManifest.AddDifferentialDrive(manifest, views, differentialDriveMessageInterpolator, robotConfigFolder);
            GyroscopeMessageInterpolatorAgent gyroscopeMessageInterpolator = NavigationManifest.AddGyroscopeMessageInterpolator(manifest, views, heartbeat, kinect);
            VirtualDriveFeedbackAgent driveFeedback = NavigationManifest.AddVirtualDriveFeedback(manifest, views, diffDrive, gyroscopeMessageInterpolator, robotConfigFolder);
            manifest.ActiveAgents.Add(driveFeedback.Name);
            ProxyDefinition driveFeedbackProxy = NavigationManifest.AddDriveFeedbackNetworkEnabledProxy(manifest, driveFeedback, controllerNameOrAddress);
            manifest.ActiveAgents.Add(driveFeedbackProxy.Name);

            // path tracking
            IProducer<SimpleAgentMessage<DriveDirection>> pathTracking = PathPlanningAndTrackingManifest.AddPathTrackingRemoteProxy(manifest, views, controllerNameOrAddress);
            IProducer<SimpleAgentMessage<Pose2D>> pose = NavigationManifest.AddRobotMapPoseRemoteProxy(manifest, views, controllerNameOrAddress);
            PathTrackingDriveAgent pathTrackingDrive = PathPlanningAndTrackingManifest.AddPathTrackingDrive(manifest, views, pathTracking, pose, hdp, driveFeedback);

            // local suggestion drive (obstacle avoidance)
            Ev4LocalSuggestionDriveAgent localSuggestionDriveAgent = PathPlanningAndTrackingManifest.AddEv4LocalSuggestionDrive(manifest, views, pathTrackingDrive, turretScanner, heartbeat);
            manifest.ActiveAgents.Add(localSuggestionDriveAgent.Name);

            // DifferencialDriveTracking (with remote game pad override [gamePad can be real (requires x86 on source machine) or emulated])
            IProducer<GamePadAgentMessage> gamePadAgent = InteractionManifest.AddRemoteGamePadAgentProxy(manifest, views, controllerNameOrAddress);
            manifest.ActiveAgents.Add(gamePadAgent.Name);
            DifferentialDriveTrackingAgent driveTracking = NavigationManifest.AddDifferentialDriveTrackingWithGamePadOverride(manifest, views, localSuggestionDriveAgent, heartbeat, gamePadAgent);
            manifest.ActiveAgents.Add(driveTracking.Name);

            // add robot pose estimation agents
            if (useUpFacingCamera)
            {
                IProducer<RGBCameraAgentMessage> rgbCamera = VisionManifest.AddRGBCameraAgent(manifest, views, 1, 5); // slow framerate
                ProxyDefinition rgbCameraControlProxy = PathPlanningAndTrackingManifest.AddControlProxy(manifest, rgbCamera.Name, controllerNameOrAddress, typeof(RGBCameraInputMessage), (int)RemotingPorts.RgbCameraControl);
                IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(rgbCamera, manifest, views, robotConfigFolder, environmentConfigFolder);
                ProxyDefinition cameraPoseEstimationProxy = VisionManifest.AddCameraPoseEstimationNetworkEnabledProxy(manifest, cameraPoseEstimation, controllerNameOrAddress);
                IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = NavigationManifest.AddRobotPoseEstimationAgent(rgbCamera, cameraPoseEstimation, manifest, views, robotConfigFolder);
                ProxyDefinition robotPoseEstimationProxy = NavigationManifest.AddRobotPoseEstimationNetworkEnabledProxy(manifest, robotPoseEstimation, controllerNameOrAddress);
                manifest.ActiveAgents.Add(robotPoseEstimation.Name);
                manifest.ActiveAgents.Add(robotPoseEstimationProxy.Name);
                manifest.ActiveAgents.Add(cameraPoseEstimationProxy.Name);
                manifest.ActiveAgents.Add(rgbCameraControlProxy.Name);
                RuntimeManifest.AddRecorder<RGBCameraAgentMessage>(rgbCamera, manifest);
            }

            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Creates a manifest for playing back recorded sensor data from the robot.
        /// </summary>
        /// <remarks>This manifest may be run as if it were the remote robot; paired with PathTracking*Manifest.</remarks>
        /// <param name="controllerNameOrAddress">The name or IP address of the machine to broadcast to (e.g. a desktop machine)</param>
        /// <param name="recordingsPath">Path to recording files.</param>
        /// <returns>A manifest to run path tracking playback.</returns>
        public static Manifest CreatePathTrackingPlaybackManifest(string controllerNameOrAddress, string recordingsPath)
        {
            Manifest manifest = new Manifest();
            var views = new List<ViewDefinition>();

            TimerAgent timer = RuntimeManifest.AddPlaybackTimer(manifest, views);

            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinect = RuntimeManifest.AddPlayer<DepthCameraAgentMessage<KinectDepthCameraParameters>>(timer, manifest, recordingsPath);

            manifest.ActiveAgents.Add(
                VisionManifest.AddKinectRawNetworkEnabledProxy(
                    manifest,
                    kinect,
                    controllerNameOrAddress).Name);

            manifest.ActiveAgents.Add(
                NavigationManifest.AddDriveFeedbackNetworkEnabledProxy(
                    manifest,
                    RuntimeManifest.AddPlayer<SimpleAgentMessage<DriveFeedbackState>>(timer, manifest, recordingsPath),
                    controllerNameOrAddress).Name);

            manifest.ActiveAgents.Add(
                NavigationManifest.AddRobotPoseEstimationNetworkEnabledProxy(
                    manifest,
                    RuntimeManifest.AddPlayer<RobotPoseEstimationAgentMessage>(timer, manifest, recordingsPath),
                    controllerNameOrAddress).Name);

            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);

            return manifest;
        }

        /// <summary>
        /// Adds a desktop proxy for control of the turret scanner agent running on the robot.
        /// </summary>
        /// <param name="manifest">The manifest to add to.</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot</param>
        /// <returns>The proxy</returns>
        public static IControllable<TurretScanInitiationMessage> AddTurretScannerRemoteProxy(Manifest manifest, string robotNameOrAddress)
        {
            // now create a proxy with the same name and add it to the manifest
            ProxyDefinition turretScannerControlProxy = ProxyDefinition.CreateRemoteProxy(
                name: "TurretScannerControl",
                address: robotNameOrAddress,
                publishContracts: null,
                publishNetworkPortNumber: 0,
                controlContracts: new[] { typeof(TurretScanInitiationMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.TurretScannerControl);
            manifest.RemoteAgentProxies.Add(turretScannerControlProxy);

            return turretScannerControlProxy.AsControllable<TurretScanInitiationMessage>();
        }

        /// <summary>
        /// Creates a manifest for running path tracking with metrics on the controller.
        /// </summary>
        /// <param name="navWaypointsFile">The nav waypoints file generated during SLAM</param>
        /// <param name="landmarkIds">Array of landmark Ids to visit in order</param>
        /// <param name="robotNameOrAddress">The name or IP address of the robot to broadcast to which to connect.</param>
        /// <param name="robotConfigFolder">Folder in which to find calibration files.</param>
        /// <param name="mapFile">The map to load.</param>
        /// <param name="startPose">The start pose of the robot.</param>
        /// <param name="startPoseVariance">The ambiguity in the start pose.</param>
        /// <param name="navBenchmarkHost">Optional.  Host machine running the Nav Benchmark .  This will modify the behavior of the state machine to match nav benchmark procedures.</param>
        /// <param name="navBenchmarkStartLandmarkId">Optional landmark ID where the nav benchmark starts</param>
        /// <returns>A manifest to run path planning with metrics on the controller (desktop/laptop).</returns>
        public static Manifest CreatePathPlanningMetricsManifest(string navWaypointsFile, int[] landmarkIds, string robotNameOrAddress, string robotConfigFolder, string mapFile, Pose2D startPose, Pose2D startPoseVariance,  string navBenchmarkHost = null, int navBenchmarkStartLandmarkId = 0)
        {
            return PathPlanningAndTrackingManifest.CreatePathPlanningManifest(
                robotNameOrAddress,
                robotConfigFolder,
                (manifest, views, hdp, driveFeedback) =>
                    NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(manifest, views, hdp, driveFeedback, mapFile, startPose, startPoseVariance, robotConfigFolder),
                (manifest, views, pathPlanning, driveFeedback, battery) =>
                {
                    PathPlanningAndTrackingMetricsAgent metrics = new PathPlanningAndTrackingMetricsAgent(
                        name: "PathPlanningAndTrackingMetricsAgent",
                        driveFeedbackProducer: driveFeedback,
                        pathPlanProducer: pathPlanning);
                    manifest.Agents.Add(metrics);

                    PathTrackingAgent pathTracking = PathPlanningAndTrackingManifest.AddPathTracking(manifest, views, pathPlanning);

                    IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationRemoteProxy(manifest, robotNameOrAddress);
                    IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = NavigationManifest.AddRobotPoseEstimationRemoteProxy(manifest, robotNameOrAddress);
                    IControllable<RGBCameraInputMessage> rgbCameraControl = VisionManifest.RGBCameraControlRemoteProxy(manifest, robotNameOrAddress);
                    IControllable<TurretScanInitiationMessage> turretScannerControl = PathPlanningAndTrackingManifest.AddTurretScannerRemoteProxy(manifest, robotNameOrAddress);

                    PathPlanningAndTrackingMetricsControllerAgent pathTrackingController =
                        new PathPlanningAndTrackingMetricsControllerAgent(
                            name: "PathPlanningAndTrackingMetricsControllerAgent",
                            localizationMetricsLogFilePath: @"C:\Metrics\LocMetrics.csv",
                            navMetricsLogFilePath: @"C:\Metrics\NavMetrics.csv",
                            minimumRobotPoseSamplesPerWaypoint: 2,
                            robotPoseSampleConsensusThresholdInMillimeters: 10,
                            batteryLowThreshold: 50,
                            waypointArrivalThreshold: 0.10,
                            mapResolution: GlobalMap.DefaultDistanceResolution,
                            navWaypointsFileName: navWaypointsFile,
                            landmarkIds: landmarkIds,
                            startPose: startPose,
                            measureGroundTruth: navBenchmarkHost == null, // do not measure ground truth if using the nav benchmark
                            robotName: robotNameOrAddress,
                            driveDirectionProducer: pathTracking,
                            metricsProducer: metrics,
                            batteryProducer: battery,
                            driveFeedbackProducer: driveFeedback,
                            cameraPoseEstimationProducer: cameraPoseEstimation,
                            robotPoseEstimationProducer: robotPoseEstimation,
                            rgbCameraControl: rgbCameraControl,
                            turretScannerControl: turretScannerControl,
                            benchmarkServerHostname: navBenchmarkHost,
                            benchmarkServerPort: 7576,
                            benchmarkStartingLandmarkId: navBenchmarkStartLandmarkId);
                    manifest.Agents.Add(pathTrackingController);
                    manifest.ActiveAgents.Add(pathTrackingController.Name);

                    RuntimeManifest.AddRecorder<BatteryMessage>(battery, manifest);
                    RuntimeManifest.AddRecorder<CameraPoseEstimationAgentMessage>(cameraPoseEstimation, manifest);
                    RuntimeManifest.AddRecorder<RobotPoseEstimationAgentMessage>(robotPoseEstimation, manifest);

                    return pathTrackingController;
                },
                true);
        }

        /// <summary>
        /// Generate random walk of a set of given waypoints.
        /// </summary>
        /// <param name="array">Set of waypoint from which to choose.</param>
        /// <param name="seed">Initial random seed value.</param>
        /// <returns>Infinite sequence of waypoints chosen at random.</returns>
        /// <typeparam name="T">The type of the elements in this sequence</typeparam>
        public static IEnumerable<T> GenerateRandomSequence<T>(T[] array, int seed)
        {
            return FilterRepeats(RandSeq(seed).Select(r => r % array.Length)).Select(r => array[r]);
        }

        /// <summary>
        /// Generate random walk of cement loop waypoints, followed by the -2 which is the dock index.
        /// </summary>
        /// <param name="count">Number of points to walk (plus the dock).</param>
        /// <param name="seed">Initial random seed.</param>
        /// <returns>Infinite sequence of</returns>
        public static int[] GenerateCementLoopLandmarkIds(int count, int seed)
        {
            return GenerateRandomSequence(new int[] { 0, 1, 2, 3, 4, 5, 6, 7 }, seed).Take(count).Concat(new int[] { -2 }).ToArray();
        }

        /// <summary>
        /// Generate random walk of 115/4 loop waypoints, followed by the -2 which is the dock index.
        /// These happen to currently be the same as the cement loop.
        /// </summary>
        /// <param name="count">Number of points to walk (plus the dock).</param>
        /// <param name="seed">Initial random seed.</param>
        /// <returns>Infinite sequence of</returns>
        public static int[] GenerateBldg115Floor4LandmarkIds(int count, int seed)
        {
            return GenerateRandomSequence(new int[] { 0, 1, 2, 3, 4, 5, 6, 7 }, seed).Take(count).Concat(new int[] { -2 }).ToArray();
        }
        
        /// <summary>
        /// Creates a manifest for running path tracking with metrics on the controller.
        /// </summary>        
        /// <param name="landmarkIds">landmark IDs to traverse.</param>
        /// <param name="environmentConfigFolder">Path to map file and waypoints.</param>
        /// <param name="iterationCount">The number of times to run the recording.</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="robotNameOrAddress">Name of robot being run.</param>
        /// <param name="calibrationFolder">Folder in which to find calibration files.</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs.</param>
        /// <returns>A manifest to run path planning with metrics on the controller (desktop/laptop).</returns>
        public static Manifest CreateCementLoopMetricsManifest(int[] landmarkIds, string environmentConfigFolder, int iterationCount, Pose2D startPose, string robotNameOrAddress, string calibrationFolder, bool addUI = true)
        {
            return CreateNavMetricsManifest(
                Path.Combine(environmentConfigFolder, NavWaypointsFromSlamAgent.WaypointsFileName),
                landmarkIds,
                iterationCount,
                startPose,
                new Pose2D(5, 5, 0.1),
                robotNameOrAddress,
                calibrationFolder,
                Path.Combine(environmentConfigFolder, "BigCementMap.xml"),
                addUI);
        }

        /// <summary>
        /// Creates a manifest for running path tracking with metrics on the controller.
        /// </summary>        
        /// <param name="landmarkIds">Array of landmarks to visit in order.</param>
        /// <param name="environmentConfigFolder">Path to map file and waypoints.</param>
        /// <param name="iterationCount">The number of times to run the recording.</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="robotNameOrAddress">Name of robot being run.</param>
        /// <param name="calibrationFolder">Folder in which to find calibration files.</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs.</param>
        /// <returns>A manifest to run path planning with metrics on the controller (desktop/laptop).</returns>
        public static Manifest Create4thFloorMetricsManifest(int[] landmarkIds, string environmentConfigFolder, int iterationCount, Pose2D startPose, string robotNameOrAddress, string calibrationFolder, bool addUI = true)
        {
            return CreateNavMetricsManifest(
                Path.Combine(environmentConfigFolder, NavWaypointsFromSlamAgent.WaypointsFileName),
                landmarkIds,
                iterationCount,
                startPose,
                new Pose2D(5, 5, 0.1),
                robotNameOrAddress,
                calibrationFolder,
                Path.Combine(environmentConfigFolder, "MapFullCroppedAndRotated.xml"),
                addUI);
        }
        
        /// <summary>
        /// Creates a manifest for running end-to-end path planning and tracking with metrics on the robot, no remoting.
        /// </summary>
        /// <param name="landmarkIds">Waypoints to traverse.</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="robotConfigFolder">Ev4 calibration folder path.</param>
        /// <param name="environmentConfigFolder">Environment config folder path</param>
        /// <param name="depthFormat">Depth image format.</param>
        /// <param name="iterationCount">The number of times to run the recording.</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs.</param>
        /// <returns>A manifest to run path planning with metrics completely on the robot.</returns>
        public static Manifest CreateIndependentCementLoopMetricsManifest(int[] landmarkIds, Pose2D startPose, string robotConfigFolder, string environmentConfigFolder, DepthImageFormat depthFormat, int iterationCount, bool addUI = true)
        {
            var mapFile = Path.Combine(environmentConfigFolder, "BigCementMap.xml");
            var startPoseVariance = new Pose2D(5, 5, 0.1);

            Manifest manifest = new Manifest();
            var views = new List<ViewDefinition>();

            HeartbeatAgent heartbeat = EV4Manifest.AddEV4HeartbeatAgent(manifest, views, robotConfigFolder);
            KinectAgent kinectRaw = VisionManifest.AddEV4Kinect(manifest, views, depthFormat, true);
            TurretScannerAgent turretScanner = NavigationManifest.AddTurretScanner(manifest, views, robotConfigFolder, heartbeat, kinectRaw);
            KinectAgentMessageProcessor kinect = VisionManifest.AddKinectProcessor(manifest, views, turretScanner, robotConfigFolder, 1000);
            KinectHorizontalDepthProfileAgent hdp = NavigationManifest.AddKinectHorizontalDepthProfile(manifest, views, kinect, robotConfigFolder);
            manifest.ActiveAgents.Add(hdp.Name);
            DifferentialDriveAgent diffDrive = NavigationManifest.AddDifferentialDrive(manifest, views, heartbeat, robotConfigFolder);
            VirtualDriveFeedbackAgent driveFeedback = NavigationManifest.AddVirtualDriveFeedback(manifest, views, diffDrive, heartbeat, robotConfigFolder);
            IProducer<SLAM2DAgentMessage> localization = NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(manifest, views, hdp, driveFeedback, mapFile, startPose, startPoseVariance, robotConfigFolder);

            manifest.ActiveAgents.Add(driveFeedback.Name);

            PathPlanningAgent pathPlanning = PathPlanningAndTrackingManifest.AddPathPlanning(
                manifest: manifest,
                views: views,
                radiusInMeters: 0.2, // padded to allow for path deviation
                minObstacleSizeInMeters: 0.1,
                maxObstacleDistanceInMeters: 5.0,
                planner: new PathPlanner(),
                mapProducer: localization);

            IProducer<SimpleAgentMessage<DriveDirection>> pathTracking = PathPlanningAndTrackingManifest.AddPathTracking(manifest, views, pathPlanning);
            manifest.ActiveAgents.Add(pathTracking.Name);

            PathPlanningAndTrackingMetricsAgent metrics = new PathPlanningAndTrackingMetricsAgent(
                name: "PathPlanningAndTrackingMetricsAgent",
                driveFeedbackProducer: driveFeedback,
                pathPlanProducer: pathPlanning);
            manifest.Agents.Add(metrics);

            RGBCameraAgent rgbCamera = VisionManifest.AddRGBCameraAgent(manifest, views, 1, 5); // slow framerate
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationAgent(rgbCamera, manifest, views, robotConfigFolder, environmentConfigFolder);
            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = NavigationManifest.AddRobotPoseEstimationAgent(rgbCamera, cameraPoseEstimation, manifest, views, robotConfigFolder);
            manifest.ActiveAgents.Add(robotPoseEstimation.Name);

            PathPlanningAndTrackingMetricsControllerAgent pathTrackingController =
                new PathPlanningAndTrackingMetricsControllerAgent(
                    name: "PathPlanningAndTrackingMetricsControllerAgent",
                    localizationMetricsLogFilePath: @"C:\Metrics\LocMetrics.csv",
                    navMetricsLogFilePath: @"C:\Metrics\NavMetrics.csv",
                    minimumRobotPoseSamplesPerWaypoint: 2,
                    robotPoseSampleConsensusThresholdInMillimeters: 10,
                    batteryLowThreshold: 50,
                    waypointArrivalThreshold: 0.10,
                    mapResolution: GlobalMap.DefaultDistanceResolution,
                    navWaypointsFileName: Path.Combine(environmentConfigFolder, NavWaypointsFromSlamAgent.WaypointsFileName),
                    landmarkIds: landmarkIds,
                    startPose: startPose,
                    measureGroundTruth: true,
                    robotName: "$robot",
                    driveDirectionProducer: pathTracking,
                    metricsProducer: metrics,
                    batteryProducer: heartbeat,
                    driveFeedbackProducer: driveFeedback,
                    cameraPoseEstimationProducer: cameraPoseEstimation,
                    robotPoseEstimationProducer: robotPoseEstimation,
                    rgbCameraControl: rgbCamera,
                    turretScannerControl: turretScanner,
                    benchmarkServerHostname: null,
                    benchmarkServerPort: 7576,
                    benchmarkStartingLandmarkId: 0);
            manifest.Agents.Add(pathTrackingController);
            manifest.ActiveAgents.Add(pathTrackingController.Name);

            SlamPoseToMapPoseAgent robotPose = new SlamPoseToMapPoseAgent("RobotMapPoseAgent", localization);
            manifest.Agents.Add(robotPose);
            manifest.ActiveAgents.Add(robotPose.Name);
            PathTrackingDriveAgent pathTrackingDrive = PathPlanningAndTrackingManifest.AddPathTrackingDrive(manifest, views, pathTrackingController, robotPose, hdp, driveFeedback);
            Ev4LocalSuggestionDriveAgent localSuggestionDriveAgent = PathPlanningAndTrackingManifest.AddEv4LocalSuggestionDrive(manifest, views, pathTrackingDrive, turretScanner, heartbeat);
            manifest.ActiveAgents.Add(localSuggestionDriveAgent.Name);
            DifferentialDriveTrackingAgent driveTracking = NavigationManifest.AddDifferentialDriveTracking(manifest, views, localSuggestionDriveAgent, heartbeat);
            manifest.ActiveAgents.Add(driveTracking.Name);

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: pathTrackingController,
                completionAgents: new[] { pathTrackingController },
                iterations: iterationCount); 
            manifest.Agents.Add(controller);
            manifest.ActiveAgents.Add(controller.Name);

            if (addUI)
            {
                UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
                manifest.Agents.Add(userInterface);
                manifest.ActiveAgents.Add(userInterface.Name);
            }

            ////RuntimeManifest.AddRecorder<KinectAgentRawMessage>(kinectRawProxy, manifest);
            ////RuntimeManifest.AddRecorder<SimpleAgentMessage<DriveFeedbackState>>(driveFeedbackProxy, manifest);
            ////RuntimeManifest.AddRecorder<SimpleAgentMessage<DriveDirection>>(pathTracking, manifest);
            ////RuntimeManifest.AddRecorder<SLAM2DAgentMessage>(localization, manifest);

            return manifest;
        }

        /// <summary>
        /// Generate random number, given the current seed.
        /// </summary>
        /// <remarks>
        /// Not using System.Random here because this algorithm is shared with native C++.
        /// See Knuth - Semi-numerical Algorithms pg. 10 
        /// </remarks>
        /// <param name="seed">Current seed. Perhaps initial value or last random number.</param>
        /// <returns>Random positive integer.</returns>
        internal static int Rand(int seed)
        {
            const int M = 2147483647;
            const int A = 16807;
            const int Q = M / A;
            const int R = M % A;
            var n = A * (seed % Q) - R * (seed / Q);
            return n <= 0 ? n + M : n;
        }

        /// <summary>
        /// Generate an infinite sequence of random numbers, beginning with seed.
        /// </summary>
        /// <param name="seed">Initial seed value (first value returned as well).</param>
        /// <returns>Infinite sequence of random positive integers.</returns>
        internal static IEnumerable<int> RandSeq(int seed)
        {
            while (true)
            {
                seed = Rand(seed);
                yield return seed;
            }
        }

        /// <summary>
        /// Filter directly repeating values from a sequence.
        /// </summary>
        /// <remarks>That is, A B B C D D D E F... becomes A B C D E F...</remarks>
        /// <param name="seq">Sequence of values</param>
        /// <returns>Sequence with repeats filtered out.</returns>
        internal static IEnumerable<int> FilterRepeats(IEnumerable<int> seq)
        {
            // zip into pairs, remove "twins" and return first of each pair
            return seq.Zip(seq.Skip(1), ((x, y) => new { x, y })).Where(p => p.x != p.y).Select(p => p.x);
        }

        /// <summary>
        /// Creates a manifest for running path tracking with metrics on the controller.
        /// </summary>
        /// <param name="navWaypointsFile">Path to nav waypoints file.</param>
        /// <param name="landmarkIds">The landmarks Ids to visit</param>
        /// <param name="iterationCount">The number of times to run the recording.</param>
        /// <param name="startPose">The start pose of the robot, relative to the bottom right corner of the map, in map units.</param>
        /// <param name="startPoseVariance">Starting pose variance.</param>
        /// <param name="robotNameOrAddress">Name of robot being run.</param>
        /// <param name="robotConfigFolder">Folder in which to find calibration files.</param>
        /// <param name="mapFile">Map file to use.</param>
        /// <param name="addUI">If true, the UI agent is added to the pipeline. Needed for automated metrics runs.</param>
        /// <returns>A manifest to run path planning with metrics on the controller (desktop/laptop).</returns>
        private static Manifest CreateNavMetricsManifest(string navWaypointsFile, int[] landmarkIds, int iterationCount, Pose2D startPose, Pose2D startPoseVariance, string robotNameOrAddress, string robotConfigFolder, string mapFile, bool addUI = true)
        {
            return PathPlanningAndTrackingManifest.CreatePathPlanningManifest(
                robotNameOrAddress,
                robotConfigFolder,
                (manifest, views, hdp, driveFeedback) =>
                    NavigationManifest.AddStaticMapLocalization<DriveFeedbackState>(manifest, views, hdp, driveFeedback, mapFile, startPose, startPoseVariance, robotConfigFolder),
                (manifest, views, pathPlanning, driveFeedback, battery) =>
                {
                    PathPlanningAndTrackingMetricsAgent metrics = new PathPlanningAndTrackingMetricsAgent(
                        name: "PathPlanningAndTrackingMetricsAgent",
                        driveFeedbackProducer: driveFeedback,
                        pathPlanProducer: pathPlanning);
                    manifest.Agents.Add(metrics);

                    PathTrackingAgent pathTracking = PathPlanningAndTrackingManifest.AddPathTracking(manifest, views, pathPlanning);

                    IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimation = VisionManifest.AddCameraPoseEstimationRemoteProxy(manifest, robotNameOrAddress);
                    IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimation = NavigationManifest.AddRobotPoseEstimationRemoteProxy(manifest, robotNameOrAddress);
                    IControllable<RGBCameraInputMessage> rgbCameraControl = VisionManifest.RGBCameraControlRemoteProxy(manifest, robotNameOrAddress);
                    IControllable<TurretScanInitiationMessage> turretScannerControl = PathPlanningAndTrackingManifest.AddTurretScannerRemoteProxy(manifest, robotNameOrAddress);

                    PathPlanningAndTrackingMetricsControllerAgent pathTrackingController =
                        new PathPlanningAndTrackingMetricsControllerAgent(
                            name: "PathPlanningAndTrackingMetricsControllerAgent",
                            localizationMetricsLogFilePath: @"C:\Metrics\LocMetrics.csv",
                            navMetricsLogFilePath: @"C:\Metrics\NavMetrics.csv",
                            minimumRobotPoseSamplesPerWaypoint: 2,
                            robotPoseSampleConsensusThresholdInMillimeters: 10,
                            batteryLowThreshold: 50,
                            waypointArrivalThreshold: 0.10,
                            mapResolution: GlobalMap.DefaultDistanceResolution,
                            navWaypointsFileName: navWaypointsFile,
                            landmarkIds: landmarkIds,
                            startPose: startPose,
                            measureGroundTruth: true,
                            robotName: robotNameOrAddress,
                            driveDirectionProducer: pathTracking,
                            metricsProducer: metrics,
                            batteryProducer: battery,
                            driveFeedbackProducer: driveFeedback,
                            cameraPoseEstimationProducer: cameraPoseEstimation,
                            robotPoseEstimationProducer: robotPoseEstimation,
                            rgbCameraControl: rgbCameraControl,
                            turretScannerControl: turretScannerControl,
                            benchmarkServerHostname: null,
                            benchmarkServerPort: 7576,
                            benchmarkStartingLandmarkId: 0);
                    manifest.Agents.Add(pathTrackingController);
                    manifest.ActiveAgents.Add(pathTrackingController.Name);

                    MetricsController controller = new MetricsController(
                        name: "controller",
                        calculatorAgent: pathTrackingController,
                        completionAgents: new[] { pathTrackingController },
                        iterations: iterationCount); 
                  
                    manifest.Agents.Add(controller);
                    manifest.ActiveAgents.Add(controller.Name);

                    return pathTrackingController;
                },
                addUI);
        }

        /// <summary>
        /// Add downscaling agent and proxy
        /// </summary>
        /// <param name="manifest">The manifest</param>
        /// <param name="kinect">Kinect agent</param>
        /// <param name="controllerNameOrAddress">Controller machine name</param>        
        /// <param name="resolutionDownScale">Resolution down scaling</param>
        /// <param name="frequencyDownScale">Frequency down scaling</param>
        private static void AddDepthFrameDownscalerAgent(
            Manifest manifest, 
            IProducer<DepthCameraAgentMessage<KinectDepthCameraParameters>> kinect, 
            string controllerNameOrAddress,
            int resolutionDownScale,
            int frequencyDownScale)
        {
            KinectFrameDownScalingAgent depthFrameDownscalerAgent = VisionManifest.AddKinectDownscalerAgent(manifest, kinect, resolutionDownScale, frequencyDownScale);
            manifest.ActiveAgents.Add(depthFrameDownscalerAgent.Name);
            ProxyDefinition depthFrameDownscaleProxy = VisionManifest.AddDepthFrameDownScalingNetworkEnabledProxy(manifest, depthFrameDownscalerAgent, controllerNameOrAddress);
            manifest.ActiveAgents.Add(depthFrameDownscaleProxy.Name);
        }
    }
}
